// Author : Zhuowen Tu
#include "mcmcParticle.h"
#include "mcEdgeOp.h"

/////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// construction & destruction
MCMC_Particle::MCMC_Particle()
{
	pmx_saliency_map = NULL;
}

MCMC_Particle::~MCMC_Particle()
{
	Free();
}

void MCMC_Particle::Free()
{
	if (pmx_saliency_map != NULL)
		delete []pmx_saliency_map;
	pmx_saliency_map = NULL;
}

int	MCMC_Particle::ChooseParticle(mcmcVector<float> &votes)
{
	rf_model.Set(votes);
	return (int)rf_model.Sampling();
}

double MCMC_Particle::GetPdf(mcmcVector<float> &votes, mcmcVector<double> &means)
{
	double	pdf;
	int		icluster;

	icluster = ClosestParticle(means);
	// find the one in the clusters that is closest to the given mean
	// set the pdf function of choosing the particle
	rf_model.Set(votes);
	pdf = rf_model.pdf(icluster);

	return pdf;
}

void MCMC_Particle::AddClusterVote(mcmcVector<float> &vc_vote, const int j, const int i, const int isubpar)
{
	for (int k=0; k<num_particle; k++)
		vc_vote(k) += pmx_saliency_map[k](j,i);
}

//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// Particels  in L,U,V space

MCMC_Particle_LUV::MCMC_Particle_LUV()
:MCMC_Particle()
{
	prand_vectors = NULL;
	pmeans = NULL;
}

MCMC_Particle_LUV::~MCMC_Particle_LUV()
{
	Free();
}

void MCMC_Particle_LUV::Free(void)
{
	MCMC_Particle::Free();

	if (prand_vectors!=NULL)
		delete []prand_vectors;
	prand_vectors = NULL;

	if (pmeans!=NULL)
		delete []pmeans;
	pmeans = NULL;
}

void MCMC_Particle_LUV::Set(MCMC_FilterResp &respFilters, mcmcMatrix<int> &clst, char *pfile_name)
{
	// initialization
	prespFilters = &respFilters;

	mx_map_temp = clst;
	num_particle = mx_map_temp.GetMax()+1;

	// compute variance and mean of each cluster
	mcmcMatrix<int>		matrix_temp=mx_map_temp+1;
	mcmcMatrix<int>	counts;
	mcmcMatrix<double>	*pvars;
	int				i;

	Free();

	pmeans = new mcmcVector<double>[num_particle];
	prand_vectors = new irmm_RandVector[num_particle];
	pvars = new mcmcMatrix<double>[num_particle];

	irmm_Rect	bound;
	bound.Set(0,0,prespFilters->cols()-1,prespFilters->rows()-1);

	prespFilters->image_original.MeanVariance(matrix_temp,bound,num_particle,pmeans,pvars,counts);

	for (i=0; i<num_particle; i++)
		prand_vectors[i].Set(pmeans[i],pvars[i]);

	delete []pvars;
}

mcmcVector<double> MCMC_Particle_LUV::GetMean(const int iparticle)
{
	return pmeans[iparticle];
}

int	MCMC_Particle_LUV::ClosestParticle(mcmcVector<double> &mx_mean)
{
	double			min_dist,dist_temp;
	mcmcMatrix<double>	mx_par;
	int				i,ipar=-1;

	min_dist = 1e10;

	for (i=0; i<num_particle; i++)
	{
		mx_par = GetMean(i);
		dist_temp = sqrt(mx_par.Dist(mx_mean));
		if (dist_temp<min_dist)
		{
			ipar = i;
			min_dist = dist_temp;
		}
	}
	return ipar;
}

int	MCMC_Particle_LUV::CloserParticle(const mcmcVector<double> &mx_mean1, const mcmcVector<double> &mx_mean2, const int ipar)
{
	double dist1,dist2;

	assert(ipar>=0 && ipar<num_particle);
	dist1 = pmeans[ipar].Dist(mx_mean1);
	dist2 = pmeans[ipar].Dist(mx_mean2);

	if (dist1<=dist2)
		return 0;
	else
		return 1;
}

void MCMC_Particle_LUV::Diffuse(mcmcVector<double> &mx_mean, const int ipar)
{
	assert(ipar>=0 && ipar<num_particle);

	prand_vectors[ipar].Diffuse(mx_mean);
}


//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// Particels  in 2D color
MCMC_Particle_2DColor::MCMC_Particle_2DColor()
:MCMC_Particle()
{
	pvc_N = NULL;
}

MCMC_Particle_2DColor::~MCMC_Particle_2DColor()
{
	Free();
}

void MCMC_Particle_2DColor::Free(void)
{
	MCMC_Particle::Free();

	if (pvc_N!=NULL)
		delete []pvc_N;
	pvc_N = NULL;
}

void MCMC_Particle_2DColor::Set(MCMC_FilterResp &respFilters, mcmcMatrix<int> &clst, char *pfile_name)
{
	// initialization
	prespFilters = &respFilters;

	mx_map_temp = clst;
	num_particle = mx_map_temp.GetMax()+1;

	// compute variance and mean of each cluster
	Free();

	pvc_N = new mcmcVector<double>[num_particle];

	irmm_Rect	bound;
	bound.Set(0,0,prespFilters->cols()-1,prespFilters->rows()-1);

	prespFilters->image_original.Compute2DColor(mx_map_temp,bound,num_particle,pvc_N);
}

mcmcVector<double> MCMC_Particle_2DColor::GetMean(const int iparticle)
{
	assert(iparticle>=0 && iparticle<num_particle);

	return pvc_N[iparticle];
}

int	MCMC_Particle_2DColor::ClosestParticle(mcmcVector<double> &mx_N)
{
	double			min_dist,dist_temp;
	mcmcMatrix<double>	mx_par;
	int				i,ipar=-1;

	min_dist = 1e10;

	for (i=0; i<num_particle; i++)
	{
		mx_par = pvc_N[i];
		dist_temp = sqrt(mx_par.Dist(mx_N));
		if (dist_temp<min_dist)
		{
			ipar = i;
			min_dist = dist_temp;
		}
	}
	return ipar;
}

int	MCMC_Particle_2DColor::CloserParticle(const mcmcVector<double> &mx_mean1, const mcmcVector<double> &mx_mean2, const int ipar)
{
	double dist1,dist2;

	assert(ipar>=0 && ipar<num_particle);
	dist1 = pvc_N[ipar].Dist(mx_mean1);
	dist2 = pvc_N[ipar].Dist(mx_mean2);

	if (dist1<=dist2)
		return 0;
	else
		return 1;
}

void MCMC_Particle_2DColor::Diffuse(mcmcVector<double> &mx_N1, const int ipar)
{
	mcmcVector<double>	mx_return,mx_delta;
	int				ipar_close,i=0;

	assert(ipar>=0 && ipar<num_particle);
	mx_return = mx_N1;
	mx_delta = (pvc_N[ipar]-mx_return)*0.01;
	ipar_close = ClosestParticle(mx_return);
	while (ipar_close!=ipar && i<100)
	{
		mx_return = mx_return+mx_delta;
		mx_return = mx_return/mx_return.Norm(2.0);

		ipar_close = ClosestParticle(mx_return);
		i++;
	}
	if (ipar_close!=ipar)
		mx_return = pvc_N[ipar];

	mx_N1 = mx_return;
}

//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// Particels  in 3D color

// construction & destruction
MCMC_Particle_3DColor::MCMC_Particle_3DColor()
:MCMC_Particle()
{
	pvc_N = NULL;		// normal direction of beta surface
}

MCMC_Particle_3DColor::~MCMC_Particle_3DColor()
{
	Free();
}

void MCMC_Particle_3DColor::Free(void)
{
	MCMC_Particle::Free();

	if (pvc_N!=NULL)
		delete []pvc_N;
	pvc_N = NULL;
}

void MCMC_Particle_3DColor::Set(MCMC_FilterResp &respFilters, mcmcMatrix<int> &clst, char *pfile_name)
{
	// initialization
	prespFilters = &respFilters;

	mx_map_temp = clst;
	num_particle = mx_map_temp.GetMax()+1;

	// compute variance and mean of each cluster
	Free();

	pvc_N = new mcmcVector<double>[num_particle];
	vc_meanintensity.SetRowVector(num_particle);

	irmm_Rect	bound;
	bound.Set(0,0,prespFilters->cols()-1,prespFilters->rows()-1);

	char	str[200];
	bool	bClusterExist=false;

	fstream _file;
	
	if (pfile_name!=NULL) // Jiadi Yang
	{
		strcpy(str,pfile_name);
		strcat(str,".cst");
		_file.open(str, ios::in);
		if (_file)
		{
			bClusterExist = true;
			_file.close();
		}
	}
	if (bClusterExist)
	{
		// load clustering result directly from file
		ReadBigMatrix(mx_map_temp,str);

		num_particle = mx_map_temp.GetMax()+1;
		// compute variance and mean of each cluster
		Free();
		pvc_N = new mcmcVector<double>[num_particle];
		vc_meanintensity.SetRowVector(num_particle);

		prespFilters->image_original.Compute2DColor(mx_map_temp,bound,num_particle,pvc_N);
		ComputeMeanIntensity();

		if (pmx_saliency_map != NULL)
			delete []pmx_saliency_map;
		pmx_saliency_map = new mcmcMatrix<float>[num_particle];
		for (int k=0; k<num_particle; k++)
		{
			sprintf(str,"%s_color_%d.smp",pfile_name,k);
			ReadBigMatrix(pmx_saliency_map[k],str);
		}
	}
	else
	{
		prespFilters->image_original.Compute2DColor(mx_map_temp,bound,num_particle,pvc_N);
		ComputeMeanIntensity();

		EM();
		EM();

		WriteBigMatrix(mx_map_temp,str);

		// soft clustering map
		int				i,j,k;
		double			dtemp,dsum;
		mcmcVector<double>	vc_color(3);

		if (pmx_saliency_map != NULL)
			delete []pmx_saliency_map;
		pmx_saliency_map = new mcmcMatrix<float>[num_particle];
		for (k=0; k<num_particle; k++)
			pmx_saliency_map[k].SetDimension(prespFilters->rows(),prespFilters->cols());

		for (j=0; j<prespFilters->rows(); j++)
			for (i=0; i<prespFilters->cols(); i++)
			{
				dsum = 0.0;
				for (k=0; k<num_particle; k++)
				{
					vc_color(0) = (double)prespFilters->image_original.R(j,i);
					vc_color(1) = (double)prespFilters->image_original.G(j,i);
					vc_color(2) = (double)prespFilters->image_original.B(j,i);
					dtemp = DistanceToAParticle(vc_color,k);
					if (dtemp>1e-6) dtemp = 1/dtemp;
					else			dtemp = 1e6;
					pmx_saliency_map[k](j,i) = (float)dtemp;

					dsum += dtemp;
				}
				for (k=0; k<num_particle; k++)
					pmx_saliency_map[k](j,i) = (float)(pmx_saliency_map[k](j,i)/dsum);
			}
		for (k=0; k<num_particle; k++)
		{
			sprintf(str,"%s_color_%d.smp",pfile_name,k);
			WriteBigMatrix(pmx_saliency_map[k],str);
		}
	}
	PrecisionNormal();
	PrecisionIntensity();
}

void MCMC_Particle_3DColor::ComputeMeanIntensity(void)
{
	int				i,j,k;
	mcmcVector<int>	vc_size;
	mcmcVector<double>	vc_temp(3);
	double			dintensity;

	// initialization
	for (i=0; i<num_particle; i++)
		vc_meanintensity(i) = 0.0;
	vc_size.SetRowVector(num_particle);
	vc_size.InitValue(0);

	// compute the major direction
	for (j=0; j<prespFilters->rows(); j++)
		for (i=0; i<prespFilters->cols(); i++)
		{
			k = mx_map_temp(j,i);
			vc_size(k)++;

			vc_temp(0) = (double)prespFilters->image_original.R(j,i);
			vc_temp(1) = (double)prespFilters->image_original.G(j,i);
			vc_temp(2) = (double)prespFilters->image_original.B(j,i);

			dintensity = fabs(vc_temp.Projection(pvc_N[k]));
			vc_meanintensity(k) += dintensity;
		}

	for (k=0; k<num_particle; k++)
	{
		if (vc_size(k)>0)
			vc_meanintensity(k) = vc_meanintensity(k)/((double)vc_size(k));
	}
}

// EM algorithm
//Step 1:  compute the probability p(l(x,y) = i ). That is
//   the probability for pixel (x,y) belonging to cluster i.
//    Based on its distance in the beta plane and covariance.
//     Draw a random sample, and assign it as the label of this pixel.
//
//Step 2:  compute the cluster center (r,g,b) based on all the pixels
//  assigned to this cluster.
//      (r,g,b) <--- (1-epsilon)(r,g,b) + epsilon*mean(r,g,b)
//     epsilon should be small enough.
//
//   That is, you may not assign (r,g,b) to the new mean imediately.
//   It need to inherit from the previous (r,g,b) result to be robust.

void MCMC_Particle_3DColor::EM(void)
{
	mcmcMatrix<int>		mx_temp;
	RF				rf;
	int				i,j,k,m;
	mcmcVector<double>	*pvc_Ntemp,vc_mean_temp,vc_color;
	double			dtemp;
	irmm_Rect		bound;

	vc_color.SetRowVector(3);
	bound.Set(0,0,prespFilters->cols()-1,prespFilters->rows()-1);
	pvc_Ntemp = new mcmcVector<double>[num_particle];

	mx_temp.SetDimension(prespFilters->rows(),prespFilters->cols());
	rf.Set(0,num_particle-1,1);

	for (m=0; m<0; m++)
	{
		// setp one:
		// assign a cluster to each pixel and sample
		for (j=0; j<prespFilters->rows(); j++)
			for (i=0; i<prespFilters->cols(); i++)
			{
				for (k=0; k<num_particle; k++)
				{
					vc_color(0) = (double)prespFilters->image_original.R(j,i);
					vc_color(1) = (double)prespFilters->image_original.G(j,i);
					vc_color(2) = (double)prespFilters->image_original.B(j,i);
					dtemp = DistanceToAParticle(vc_color,k);
					if (dtemp>1e-6)
						rf.SetOneBin(k,1/dtemp);
					else
						rf.SetOneBin(k,1e6);
				}
				k = (int)rf.SamplingNonSetCdf();
				mx_temp(j,i) = k;
			}
		// setp 2:
		prespFilters->image_original.Compute2DColor(mx_temp,bound,num_particle,pvc_Ntemp);
		for (k=0; k<num_particle; k++)
			pvc_N[k] = pvc_N[k]+(pvc_Ntemp[k]-pvc_N[k])*0.01;
		ComputeMeanIntensity();
	}

	// any two clusters are overlapping?
	mcmcVector<int> vc_sign(num_particle);
	vc_sign.InitValue(1);
	for (i=0; i<num_particle; i++)
	{
		if (vc_sign(i)==1)
		{
			vc_color = pvc_N[i]*vc_meanintensity(i);
			for (j=i+1; j<num_particle; j++)
			{
				if (vc_sign(j)==1)
				{
					if (DistanceToAParticle(vc_color,j)<20.0)
						vc_sign(j) = 0;		// tow clusters are too close
				}
			}
		}
	}
	mcmcMatrix<int> vc_index(num_particle,1);
	double		dist_temp;
	int			ipar_temp;

	m = 0;
	for (i=0; i<num_particle; i++)
		if (vc_sign(i)==1)
			vc_index(i) = m++;

	// relabel
	for (j=0; j<prespFilters->rows(); j++)
		for (i=0; i<prespFilters->cols(); i++)
		{
			dist_temp = 1e90;
			ipar_temp = -1;

			for (k=0; k<num_particle; k++)
			{
				if (vc_sign(k)==1)
				{
					vc_color(0) = (double)prespFilters->image_original.R(j,i);
					vc_color(1) = (double)prespFilters->image_original.G(j,i);
					vc_color(2) = (double)prespFilters->image_original.B(j,i);
					dtemp = DistanceToAParticle(vc_color,k);
					if (dtemp<dist_temp)
					{
						ipar_temp = k;
						dist_temp = dtemp;
					}
				}
			}
			assert(ipar_temp>=0);
			mx_map_temp(j,i) = vc_index(ipar_temp);
		}

	Free();
	num_particle = m;
	pvc_N = new mcmcVector<double>[num_particle];
	prespFilters->image_original.Compute2DColor(mx_map_temp,bound,num_particle,pvc_N);
	ComputeMeanIntensity();

	delete []pvc_Ntemp;
}

double DistanceToA3DColorMode(mcmcVector<double> &vc_color, mcmcVector<double> &vc_N, const double intensity_mean)
{
	double			dintensity,d1;
	mcmcVector<double>	vc_beta;

	dintensity = vc_color.Projection(vc_N);
	vc_beta = vc_color-vc_N*dintensity;

	dintensity = dintensity-intensity_mean;
	d1 = vc_beta.Norm(2.0);
	// intensity value should have less than that of color
	return sqrt((pow(d1,2.0)+0.7*pow(dintensity,2.0))/1.7);
}

double MCMC_Particle_3DColor::DistanceToAParticle(mcmcVector<double> &vc_color, const int ipar)
{
	return DistanceToA3DColorMode(vc_color,pvc_N[ipar],vc_meanintensity(ipar));
}

mcmcVector<double> MCMC_Particle_3DColor::GetMean(const int ipar)
{
	mcmcVector<double> mx_return;

	mx_return.SetRowVector(4);

	assert(ipar>=0 && ipar<num_particle);
	// color norm
	mx_return(0) = pvc_N[ipar](0);
	mx_return(1) = pvc_N[ipar](1);
	mx_return(2) = pvc_N[ipar](2);
	// color intensity
	mx_return(3) = vc_meanintensity(ipar);

	return mx_return;
}

void MCMC_Particle_3DColor::ToColor(const mcmcVector<double> &mx_N_intensity, mcmcVector<double> &mx_color)
{
	for (int i=0; i<3; i++)
		mx_color(i) = mx_N_intensity.Data(i)*mx_N_intensity.Data(3);
}

int MCMC_Particle_3DColor::ClosestParticle(mcmcVector<double> &mx_N_intesity)
{
	mcmcVector<double>	vc_color(3);
	int				i,ipar;
	double			dist,dist_temp=1e10;

	ToColor(mx_N_intesity, vc_color);
	for (i=0; i<num_particle; i++)
	{
		dist = DistanceToAParticle(vc_color,i);
		if (dist<dist_temp)
		{
			ipar = i;
			dist_temp = dist;
		}
	}
	return ipar;
}

int MCMC_Particle_3DColor::CloserParticle(const mcmcVector<double> &mx_N_intesity1, const mcmcVector<double> &mx_N_intesity2, const int ipar)
{
	mcmcVector<double>	vc_color1(3),vc_color2(3);
	double			dist1,dist2;

	ToColor(mx_N_intesity1, vc_color1);
	ToColor(mx_N_intesity2, vc_color2);

	dist1 = DistanceToAParticle(vc_color1,ipar);
	dist2 = DistanceToAParticle(vc_color2,ipar);
	if (dist1<=dist2)
		return 0;
	else
		return 1;
}

void MCMC_Particle_3DColor::Diffuse(mcmcVector<double> &vc_N_intensity, const int ipar)
{
	mcmcVector<double>	vc_return,vc_delta(4),vc_color(3),vc_temp(3);
	int				ipar_close,i=0;

	assert(ipar>=0 && ipar<num_particle);
	vc_return = vc_N_intensity;

	vc_delta(0) = (pvc_N[ipar](0)-vc_return(0))*0.01;
	vc_delta(1) = (pvc_N[ipar](1)-vc_return(1))*0.01;
	vc_delta(2) = (pvc_N[ipar](2)-vc_return(2))*0.01;
	vc_delta(3) = (vc_meanintensity(ipar)-vc_return(3))*0.01;

	ipar_close = ClosestParticle(vc_return);
	while (ipar_close!=ipar && i<100)
	{
		vc_return = vc_return+vc_delta;
		vc_temp.CopyContent(vc_return,3);
		vc_temp = vc_temp/vc_temp.Norm(2.0);
		vc_return.CopyContent(vc_temp,3);

		ipar_close = ClosestParticle(vc_return);
		i++;
	}
	if (ipar_close!=ipar)
	{
		vc_return(0) = pvc_N[ipar](0);
		vc_return(1) = pvc_N[ipar](1);
		vc_return(2) = pvc_N[ipar](2);
		vc_return(3) = vc_meanintensity(ipar);
	}

	int temp,j;
	for (j=0; j<3; j++)
	{
		temp = IntClose(100*vc_return(j));
		vc_return(j) = (((double)temp)/100);
	}
	vc_temp.CopyContent(vc_return,3);
	vc_temp = vc_temp/vc_temp.Norm(2.0);
	vc_return.CopyContent(vc_temp,3);
	vc_return(3) = (double)IntClose(vc_return(3));

	vc_N_intensity = vc_return;
}

void MCMC_Particle_3DColor::PrecisionNormal(void)
{
	int temp;

	for (int i=0; i<num_particle; i++)
		for (int j=0; j<pvc_N[i].size(); j++)
		{
			temp = IntClose(100*pvc_N[i](j));
			pvc_N[i](j) = (((double)temp)/100);
		}
}

void MCMC_Particle_3DColor::PrecisionIntensity(void)
{
	for (int i=0; i<num_particle; i++)
		vc_meanintensity(i) = (double)IntClose(vc_meanintensity(i));
}

//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// Particels  in 3D color

// construction & destruction
MCMC_Particle_Intensity::MCMC_Particle_Intensity()
:MCMC_Particle()
{
}

MCMC_Particle_Intensity::~MCMC_Particle_Intensity()
{
	Free();
}

void MCMC_Particle_Intensity::Free(void)
{
	MCMC_Particle::Free();
}

void MCMC_Particle_Intensity::Set(MCMC_FilterResp &respFilters, mcmcMatrix<int> &clst, char *pfile_name)
{
	// initialization
	prespFilters = &respFilters;

	mx_map_temp = clst;
	num_particle = mx_map_temp.GetMax()+1;

	// compute variance and mean of each cluster
	Free();

	vc_meanintensity.SetRowVector(num_particle);

	irmm_Rect	bound;
	bound.Set(0,0,prespFilters->cols()-1,prespFilters->rows()-1);

	char	str[200];
	bool	bClusterExist=false;
	fstream _file;

	if (pfile_name!=NULL) // Jiadi Yang
	{
		strcpy(str,pfile_name);
		strcat(str,".cst");
		_file.open(str, ios::in);
		if (_file)
		{
			bClusterExist = true;
			_file.close();
		}
	}
	if (bClusterExist)
	{
		// load clustering result directly from file
		ReadBigMatrix(mx_map_temp,str);

		num_particle = mx_map_temp.GetMax()+1;
		// compute variance and mean of each cluster
		Free();
		vc_meanintensity.SetRowVector(num_particle);

		ComputeMeanIntensity();

		if (pmx_saliency_map != NULL)
			delete []pmx_saliency_map;
		pmx_saliency_map = new mcmcMatrix<float>[num_particle];
		for (int k=0; k<num_particle; k++)
		{
			sprintf(str,"%s_color_%d.smp",pfile_name,k);
			ReadBigMatrix(pmx_saliency_map[k],str);
		}
	}
	else
	{
		ComputeMeanIntensity();

		EM();
		EM();

		WriteBigMatrix(mx_map_temp,str);

		// soft clustering map
		int				i,j,k;
		double			dtemp,dsum;
		mcmcVector<double>	vc_color(3);

		if (pmx_saliency_map != NULL)
			delete []pmx_saliency_map;
		pmx_saliency_map = new mcmcMatrix<float>[num_particle];
		for (k=0; k<num_particle; k++)
			pmx_saliency_map[k].SetDimension(prespFilters->rows(),prespFilters->cols());

		for (j=0; j<prespFilters->rows(); j++)
			for (i=0; i<prespFilters->cols(); i++)
			{
				dsum = 0.0;
				for (k=0; k<num_particle; k++)
				{
					dtemp = DistanceToAParticle(prespFilters->ImageIntensity(j,i),k);
					if (dtemp>1e-6) dtemp = 1/dtemp;
					else			dtemp = 1e6;
					pmx_saliency_map[k](j,i) = (float)dtemp;

					dsum += dtemp;
				}
				for (k=0; k<num_particle; k++)
					pmx_saliency_map[k](j,i) = (float)(pmx_saliency_map[k](j,i)/dsum);
			}
		for (k=0; k<num_particle; k++)
		{
			sprintf(str,"%s_color_%d.smp",pfile_name,k);
			WriteBigMatrix(pmx_saliency_map[k],str);
		}
	}
	PrecisionIntensity();
}

void MCMC_Particle_Intensity::ComputeMeanIntensity(void)
{
	int				i,j,k;
	mcmcVector<int>	vc_size;

	// initialization
	for (i=0; i<num_particle; i++)
		vc_meanintensity(i) = 0.0;
	vc_size.SetRowVector(num_particle);
	vc_size.InitValue(0);

	// compute the major direction
	for (j=0; j<prespFilters->rows(); j++)
		for (i=0; i<prespFilters->cols(); i++)
		{
			k = mx_map_temp(j,i);
			vc_size(k)++;

			vc_meanintensity(k) += prespFilters->ImageIntensity(j,i);
		}

	for (k=0; k<num_particle; k++)
	{
		if (vc_size(k)>0)
			vc_meanintensity(k) = vc_meanintensity(k)/((double)vc_size(k));
	}
}

// EM algorithm
//Step 1:  compute the probability p(l(x,y) = i ). That is
//   the probability for pixel (x,y) belonging to cluster i.
//    Based on its distance in the beta plane and covariance.
//     Draw a random sample, and assign it as the label of this pixel.
//
//Step 2:  compute the cluster center (r,g,b) based on all the pixels
//  assigned to this cluster.
//      (r,g,b) <--- (1-epsilon)(r,g,b) + epsilon*mean(r,g,b)
//     epsilon should be small enough.
//
//   That is, you may not assign (r,g,b) to the new mean imediately.
//   It need to inherit from the previous (r,g,b) result to be robust.

void MCMC_Particle_Intensity::EM(void)
{
	mcmcMatrix<int>		mx_temp;
	RF				rf;
	int				i,j,k,m;
	mcmcVector<double>	vc_mean_temp;
	double			dtemp;
	irmm_Rect		bound;

	bound.Set(0,0,prespFilters->cols()-1,prespFilters->rows()-1);

	mx_temp.SetDimension(prespFilters->rows(),prespFilters->cols());
	rf.Set(0,num_particle-1,1);

	for (m=0; m<0; m++)
	{
		// setp one:
		// assign a cluster to each pixel and sample
		for (j=0; j<prespFilters->rows(); j++)
			for (i=0; i<prespFilters->cols(); i++)
			{
				for (k=0; k<num_particle; k++)
				{
					dtemp = DistanceToAParticle(prespFilters->ImageIntensity(j,i),k);
					if (dtemp>1e-6)
						rf.SetOneBin(k,1/dtemp);
					else
						rf.SetOneBin(k,1e6);
				}
				k = (int)rf.SamplingNonSetCdf();
				mx_temp(j,i) = k;
			}
		// setp 2:
		ComputeMeanIntensity();
	}

	// any two clusters are overlapping?
	mcmcVector<int> vc_sign(num_particle);
	vc_sign.InitValue(1);
	for (i=0; i<num_particle; i++)
	{
		if (vc_sign(i)==1)
		{
			for (j=i+1; j<num_particle; j++)
			{
				if (vc_sign(j)==1)
				{
					if (DistanceToAParticle(vc_meanintensity(i),j)<20.0)
						vc_sign(j) = 0;		// tow clusters are too close
				}
			}
		}
	}
	mcmcMatrix<int> vc_index(num_particle,1);
	double		dist_temp;
	int			ipar_temp;

	m = 0;
	for (i=0; i<num_particle; i++)
		if (vc_sign(i)==1)
			vc_index(i) = m++;

	// relabel
	for (j=0; j<prespFilters->rows(); j++)
		for (i=0; i<prespFilters->cols(); i++)
		{
			dist_temp = 1e90;
			ipar_temp = -1;

			for (k=0; k<num_particle; k++)
			{
				if (vc_sign(k)==1)
				{
					dtemp = DistanceToAParticle(prespFilters->ImageIntensity(j,i),k);
					if (dtemp<dist_temp)
					{
						ipar_temp = k;
						dist_temp = dtemp;
					}
				}
			}
			assert(ipar_temp>=0);
			mx_map_temp(j,i) = vc_index(ipar_temp);
		}

	Free();
	num_particle = m;
	ComputeMeanIntensity();
}

double MCMC_Particle_Intensity::DistanceToAParticle(double intensity, const int ipar)
{
	return sqrt(pow(intensity-vc_meanintensity(ipar),2.0));
}

mcmcVector<double> MCMC_Particle_Intensity::GetMean(const int ipar)
{
	mcmcVector<double> mx_return;

	mx_return.SetRowVector(1);

	assert(ipar>=0 && ipar<num_particle);
	// intensity
	mx_return(0) = vc_meanintensity(ipar);

	return mx_return;
}

int MCMC_Particle_Intensity::ClosestParticle(mcmcVector<double> &vc_intesity)
{
	mcmcVector<double>	vc_color(3);
	int				i,ipar;
	double			dist,dist_temp=1e10;

	for (i=0; i<num_particle; i++)
	{
		dist = DistanceToAParticle(vc_intesity(0),i);
		if (dist<dist_temp)
		{
			ipar = i;
			dist_temp = dist;
		}
	}
	return ipar;
}

int MCMC_Particle_Intensity::CloserParticle(const mcmcVector<double> &vc_intesity1, const mcmcVector<double> &vc_intesity2, const int ipar)
{
	double			dist1,dist2;

	dist1 = DistanceToAParticle(vc_intesity1.Data(0),ipar);
	dist2 = DistanceToAParticle(vc_intesity2.Data(0),ipar);
	if (dist1<=dist2)
		return 0;
	else
		return 1;
}

void MCMC_Particle_Intensity::Diffuse(mcmcVector<double> &vc_intensity, const int ipar)
{
	mcmcVector<double>	vc_return,vc_delta(1);
	int				ipar_close,i=0;

	assert(ipar>=0 && ipar<num_particle);
	vc_return = vc_intensity;

	vc_delta(0) = (vc_meanintensity(ipar)-vc_return(0))*0.01;

	ipar_close = ClosestParticle(vc_intensity);
	while (ipar_close!=ipar && i<100)
	{
		vc_return = vc_return+vc_delta;

		ipar_close = ClosestParticle(vc_return);
		i++;
	}
	if (ipar_close!=ipar)
		vc_return(0) = vc_meanintensity(ipar);

	vc_intensity = vc_return;
}

void MCMC_Particle_Intensity::PrecisionIntensity(void)
{
	for (int i=0; i<num_particle; i++)
		vc_meanintensity(i) = (double)IntClose(vc_meanintensity(i));
}

//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// Particels  in 3D color

// construction & destruction
MCMC_Particle_3DColor2::MCMC_Particle_3DColor2()
:MCMC_Particle()
{
	pvc_luv = NULL;		// normal direction of beta surface
}

MCMC_Particle_3DColor2::~MCMC_Particle_3DColor2()
{
	Free();
}

void MCMC_Particle_3DColor2::Free(void)
{
	MCMC_Particle::Free();

	if (pvc_luv!=NULL)
		delete []pvc_luv;
	pvc_luv = NULL;
}

void MCMC_Particle_3DColor2::Set(MCMC_FilterResp &respFilters, mcmcMatrix<int> &clst, char *pfile_name)
{
	// initialization
	prespFilters = &respFilters;

	mx_map_temp = clst;
	num_particle = mx_map_temp.GetMax()+1;

	// compute variance and mean of each cluster
	Free();

	pvc_luv = new mcmcVector<double>[num_particle];

	irmm_Rect	bound;
	bound.Set(0,0,prespFilters->cols()-1,prespFilters->rows()-1);

	char	str[200];
	bool	bClusterExist=false;
	fstream _file;

	if (pfile_name!=NULL) // Jiadi Yang
	{
		strcpy(str,pfile_name);
		strcat(str,".cs2");
		_file.open(str, ios::in);
		if (_file)
		{
			bClusterExist = true;
			_file.close();
		}
	}
	if (bClusterExist)
	{
		// load clustering result directly from file
		ReadBigMatrix(mx_map_temp,str);

		num_particle = mx_map_temp.GetMax()+1;
		// compute variance and mean of each cluster
		Free();

		ComputeMean();
	}
	else
	{
		ComputeMean();
		EM();
		int nrow=mx_map_temp.rows();
		int ncol=mx_map_temp.cols();

		//WriteBigMatrix(mx_map_temp,str); Jiadi
	}

	// soft clustering map
	int				i,j,k;
	double			dtemp,dsum;
	mcmcVector<double>	vc_luv(3);

	if (pmx_saliency_map != NULL)
		delete []pmx_saliency_map;
	pmx_saliency_map = new mcmcMatrix<float>[num_particle];
	for (k=0; k<num_particle; k++)
		pmx_saliency_map[k].SetDimension(prespFilters->rows(),prespFilters->cols());

	for (j = 0; j < prespFilters->rows(); j++){
		for (i = 0; i < prespFilters->cols(); i++)
		{
			dsum = 0.0;
			for (k = 0; k<num_particle; k++)
			{
				vc_luv(0) = (double)prespFilters->ImageL(j, i);
				vc_luv(1) = (double)prespFilters->ImageU(j, i);
				vc_luv(2) = (double)prespFilters->ImageV(j, i);
				dtemp = DistanceToAParticle(vc_luv, k);
				if (dtemp>1e-6) dtemp = 1 / dtemp;
				else			dtemp = 1e6;
				pmx_saliency_map[k](j, i) = (float)dtemp;

				dsum += dtemp;
			}
			for (k = 0; k < num_particle; k++)
				pmx_saliency_map[k](j, i) = (float)(pmx_saliency_map[k](j, i) / dsum);
		}
	}
}

void MCMC_Particle_3DColor2::ComputeMean(void)
{
	int				i,j,k;
	mcmcVector<double>	vc_num;
	mcmcVector<int>	vc_size;

	// initialization
	if (pvc_luv!=NULL)
		delete []pvc_luv;
	pvc_luv = new mcmcVector<double>[num_particle];
	for (i=0; i<num_particle; i++)
	{
		pvc_luv[i].SetRowVector(3);
		pvc_luv[i].InitValue(0.0);
	}
	vc_size.SetRowVector(num_particle);
	vc_size.InitValue(0);

	for (j=0; j<prespFilters->rows(); j++)
		for (i=0; i<prespFilters->cols(); i++)
		{
			k = mx_map_temp(j,i);
			vc_size(k)++;

			pvc_luv[k](0) += prespFilters->ImageL(j,i);		// L
			pvc_luv[k](1) += prespFilters->ImageU(j,i);		// U
			pvc_luv[k](2) += prespFilters->ImageV(j,i);		// U
		}
	for (k=0; k<num_particle; k++)
		if (vc_size(k)>0)
			pvc_luv[k] = pvc_luv[k]/((double)vc_size(k));
}

void MCMC_Particle_3DColor2::ToAColorImage(Raster_Color<uchar> &image_color)
{
	mcmcVector<double> vc_color(3);
	int k,pRGB[3];
	double	pLUV[3];

	image_color.SetDimension(prespFilters->rows(),prespFilters->cols());
	for (int j=0; j<prespFilters->rows(); j++)
		for (int i=0; i<prespFilters->cols(); i++)
		{
			k = mx_map_temp(j,i);

			pLUV[0] = pvc_luv[k](0);
			pLUV[1] = pvc_luv[k](1);
			pLUV[2] = pvc_luv[k](2);

			image_color.convert_LUV_RGB(pLUV,pRGB);

			image_color.R(j,i) = (uchar)pRGB[0];
			image_color.G(j,i) = (uchar)pRGB[1];
			image_color.B(j,i) = (uchar)pRGB[2];
		}
}



// EM algorithm
//Step 1:  compute the probability p(l(x,y) = i ). That is
//   the probability for pixel (x,y) belonging to cluster i.
//    Based on its distance in the beta plane and covariance.
//     Draw a random sample, and assign it as the label of this pixel.
//
//Step 2:  compute the cluster center (r,g,b) based on all the pixels
//  assigned to this cluster.
//      (r,g,b) <--- (1-epsilon)(r,g,b) + epsilon*mean(r,g,b)
//     epsilon should be small enough.
//
//   That is, you may not assign (r,g,b) to the new mean imediately.
//   It need to inherit from the previous (r,g,b) result to be robust.

void MCMC_Particle_3DColor2::EM(void)
{
	
	mcmcMatrix<int>		mx_temp;
	RF				rf;
	int				i,j,k,m;
	mcmcVector<double>	vc_mean_temp,vc_luv;
	double			dtemp;
	irmm_Rect		bound;

	vc_luv.SetRowVector(3);
	bound.Set(0,0,prespFilters->cols()-1,prespFilters->rows()-1);

	mx_temp.SetDimension(prespFilters->rows(),prespFilters->cols());
	rf.Set(0,num_particle-1,1);

	
	for (m=0; m<10; m++)
	{
		// setp one:
		// assign a cluster to each pixel and sample
		for (j=0; j<prespFilters->rows(); j++)
			for (i=0; i<prespFilters->cols(); i++)
			{
				for (k=0; k<num_particle; k++)
				{
					vc_luv(0) = (double)prespFilters->ImageL(j,i);
					vc_luv(1) = (double)prespFilters->ImageU(j,i);
					vc_luv(2) = (double)prespFilters->ImageV(j,i);
					dtemp = DistanceToAParticle(vc_luv,k);
					
					if (dtemp>1e-6)
						rf.SetOneBin(k,1/dtemp);
					else
						rf.SetOneBin(k,1e6);
				}

				
				k = (int)rf.SamplingNonSetCdf();
				mx_temp(j,i) = k;
			}
		// setp 2:
		ComputeMean();
	}
	// any two clusters are overlapping?
	mcmcVector<int> vc_sign(num_particle);

	vc_sign.InitValue(1);
	for (i=0; i<num_particle; i++)
	{
		if (vc_sign(i)==1)
		{
			vc_luv = pvc_luv[i];
			for (j=i+1; j<num_particle; j++)
			{
				if (vc_sign(j)==1)
				{
					if (DistanceToAParticle(vc_luv,j)<10.0)
						vc_sign(j) = 0;		// tow clusters are too close
				}
			}
		}
	}
	mcmcVector<int> vc_index(num_particle);
	double		dist_temp;
	int			ipar_temp;

	m = 0;
	for (i=0; i<num_particle; i++)
		if (vc_sign(i)==1)
			vc_index(i) = m++;

	// relabel
	for (j=0; j<prespFilters->rows(); j++)
		for (i=0; i<prespFilters->cols(); i++)
		{
			dist_temp = 1e90;
			ipar_temp = -1;

			for (k=0; k<num_particle; k++)
			{
				if (vc_sign(k)==1)
				{
					vc_luv(0) = (double)prespFilters->ImageL(j,i);
					vc_luv(1) = (double)prespFilters->ImageU(j,i);
					vc_luv(2) = (double)prespFilters->ImageV(j,i);
					dtemp = DistanceToAParticle(vc_luv,k);
					if (dtemp<dist_temp)
					{
						ipar_temp = k;
						dist_temp = dtemp;
					}
				}
			}
			assert(ipar_temp>=0);
			mx_map_temp(j,i) = vc_index(ipar_temp);
		}

	Free();
	num_particle = m;
	ComputeMean();
}

double MCMC_Particle_3DColor2::GetPdf(mcmcVector<float> &votes, mcmcVector<double> &vc_mean_luv)
{
	double	pdf;
	int		icluster;

	icluster = ClosestParticle(vc_mean_luv);
	// find the one in the clusters that is closest to the given mean
	// set the pdf function of choosing the particle
	rf_model.Set(votes);
	pdf = rf_model.pdf(icluster);

	return pdf;
}

__forceinline double MCMC_Particle_3DColor2::DistanceToAParticle(mcmcVector<double> &vc_luv, const int ipar)
{
	int i;

	mcmcVector<double> vc_temp(3);

	for (i = 0; i<3; i++)
		vc_temp(i) = vc_luv(i) - pvc_luv[ipar](i);
	return  vc_temp.Norm(2.0);
	/* Jiadi
	return cv::norm(vc_luv.data, pvc_luv[ipar].data);
	*/
}


int MCMC_Particle_3DColor2::ClosestParticle(mcmcVector<double> &vc_luv)
{
	int			i,ipar;
	double		dist,dist_temp=1e10;

	for (i=0; i<num_particle; i++)
	{
		dist = DistanceToAParticle(vc_luv,i);
		if (dist<dist_temp)
		{
			ipar = i;
			dist_temp = dist;
		}
	}
	return ipar;
}

mcmcVector<double> MCMC_Particle_3DColor2::GetMean(const int iparticle)
{
	return pvc_luv[iparticle];
}

void MCMC_Particle_3DColor2::Diffuse(mcmcVector<double> &vc_luv, const int ipar)
{
	mcmcVector<double>	vc_return,vc_delta(3),vc_color(3),vc_temp(3);
	int				ipar_close,i=0;

	assert(ipar>=0 && ipar<num_particle);
	vc_return = vc_luv;

	vc_delta = (pvc_luv[ipar]-vc_return)*0.01;

	ipar_close = ClosestParticle(vc_luv);
	while (ipar_close!=ipar && i<100)
	{
		vc_return = vc_return+vc_delta;

		ipar_close = ClosestParticle(vc_return);
		i++;
	}
	if (ipar_close!=ipar)
		vc_return = pvc_luv[ipar];

	vc_luv = vc_return;
}


///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
// construction & destruction
MCMC_Histogram_Clusters::MCMC_Histogram_Clusters()
{
	kcluster	= 0;
	wsize = 0;
}

MCMC_Histogram_Clusters::MCMC_Histogram_Clusters(Raster<double> &ras_resp, mcmcMatrix<int> &mx_clustering,
												 const int k, const int size_window, const double d)
{
	Set(ras_resp,mx_clustering,k,size_window,d);
}

MCMC_Histogram_Clusters::~MCMC_Histogram_Clusters()
{
}

void MCMC_Histogram_Clusters::Set(Raster<double> &ras_resp, mcmcMatrix<int> &mx_clustering,
								  const int k, const int size_window, const double d,
								  bool bPixelHistExist)
{
	kcluster	= k;
	wsize		= size_window;
	delta		= d;

	// copy filter response
	ras_fresp = ras_resp;
	if (!bPixelHistExist)
		mx_pixel_histogram.SetDimension(ras_resp.rows(),ras_resp.cols());
	mx_cluster_histogram.SetDimension(kcluster,1);

	int i,j,m,n,k1;

	min_bin = 1e10;
	max_bin = -1e10;
	// find the minimal and maximal response
	for (j=0; j<ras_resp.rows(); j++)
		for (i=0; i<ras_resp.cols(); i++)
		{
			if (ras_fresp(j,i)>max_bin)
				max_bin = ras_fresp(j,i);
			if (ras_fresp(j,i)<min_bin)
				min_bin = ras_fresp(j,i);
		}
	// initilize histogram for each cluter
	for (i=0; i<kcluster; i++)
		mx_cluster_histogram(i).Set(min_bin,max_bin,delta);

	// set histogram centered at each pixel
	for (j=0; j<ras_resp.rows(); j++)
		for (i=0; i<ras_resp.cols(); i++)
		{
			if (!bPixelHistExist)
			{
				mx_pixel_histogram(j,i).Set(min_bin,max_bin,delta);
				for (m=-wsize/2; m<=wsize/2; m++)
					for (n=-wsize/2; n<=wsize/2; n++)
					{
						if (ras_resp.Valid(j+m,i+n))
						{
							mx_pixel_histogram(j,i).AddBin(ras_fresp(j+m,i+n)-2.0*delta,0.15);
							mx_pixel_histogram(j,i).AddBin(ras_fresp(j+m,i+n)-delta,0.3);
							mx_pixel_histogram(j,i).AddBin(ras_fresp(j+m,i+n),1.0);
							mx_pixel_histogram(j,i).AddBin(ras_fresp(j+m,i+n)+delta,0.3);
							mx_pixel_histogram(j,i).AddBin(ras_fresp(j+m,i+n)+2.0*delta,0.15);
						}
					}
				mx_pixel_histogram(j,i).Normalize();
			}
			// add to the cluster
			k1 = mx_clustering(j,i);
			mx_cluster_histogram(k1) = mx_cluster_histogram(k1)+mx_pixel_histogram(j,i);
		}
	for (i=0; i<kcluster; i++)
		mx_cluster_histogram(i).Normalize();
}


double MCMC_Histogram_Clusters::DistanceToPar(const int row_j, const int col_i, const int ipar)
{
	return mx_cluster_histogram(ipar).DistTV(mx_pixel_histogram(row_j,col_i));
}

void MCMC_Histogram_Clusters::ComputeMeanHist(SimpleMatrix<Histogram> &newmean, mcmcMatrix<int> &mx_clustering)
{
	int k1,i,j;

	// initilize histogram for each cluter
	for (i=0; i<kcluster; i++)
		newmean(i).Set(min_bin,max_bin,delta);

	// set histogram centered at each pixel
	for (j=0; j<ras_fresp.rows(); j++)
		for (i=0; i<ras_fresp.cols(); i++)
		{
			// add to the cluster
			k1 = mx_clustering(j,i);
			newmean(k1) = newmean(k1)+mx_pixel_histogram(j,i);
		}
	for (i=0; i<kcluster; i++)
		newmean(i).Normalize();
}

double	MCMC_Histogram_Clusters::DistanceBetweenPars(const int i, const int j)
{
	return mx_cluster_histogram(i).DistTV(mx_cluster_histogram(j));
}

void MCMC_Histogram_Clusters::SaveToFile(char *pname)
{
	FILE	*pfile;
	char	name[100];

	for (int i=0; i<kcluster; i++)
	{
		sprintf(name,"%s_%d.his",pname,i);
		pfile = fopen(name, "w+");
		if (pfile != NULL)
		{
			fprintf(pfile, "%.8f\n", min_bin);
			fprintf(pfile, "%.8f\n", max_bin);
			fprintf(pfile, "%.8f\n", delta);
			for (int j=0; j<mx_cluster_histogram(i).count; j++)
				fprintf(pfile, "%.8f\n", mx_cluster_histogram(i).pbins[j]);
			fclose(pfile);
		}
	}
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
MCMC_Particle_Texture::MCMC_Particle_Texture(const bool blearn)
:MCMC_Particle()
{
	// window size for computations of histograms
	window_size		= 12;

	phist_clusters = NULL;
	p_mx_energymap = NULL;

	bprint_histograms = false;

	dist_to_merge = 0.2;

	bto_learn_texture = blearn;
}


MCMC_Particle_Texture::~MCMC_Particle_Texture()
{
	Free();
}

void MCMC_Particle_Texture::Free(void)
{
	MCMC_Particle::Free();

	if (phist_clusters != NULL)		delete []phist_clusters;
	phist_clusters = NULL;

	if (p_mx_energymap != NULL)		delete []p_mx_energymap;
	p_mx_energymap = NULL;
}

double MCMC_Particle_Texture::DistanceToPar(const int row_j, const int col_i, const int ipar)
{
	int				i;
	double			dist_total=0.0;
	mcmcVector<double>	vc_dists;

	vc_dists.SetRowVector(prespFilters->TotoalNumFilterResult());

	for (i=0; i<prespFilters->TotoalNumFilterResult(); i++)
		vc_dists(i) = phist_clusters[i].DistanceToPar(row_j,col_i,ipar);

	dist_total = CombinedDist(vc_dists);

	return dist_total;
}

double	MCMC_Particle_Texture::CombinedDist(const mcmcVector<double> &vc_dist)
{
	double d=0.0;

	for (int i=0; i<vc_dist.size(); i++)
		d += pow(vc_dist.Data(i)*vc_weights(i),2.0);
	// set a proper distance measure
	return sqrt(d/((double)vc_dist.size())*4.0);
}

void MCMC_Particle_Texture::SavePixelHistogram(const int row, const int col, char *pname)
{
	FILE	*pfile;
	char	name[100];

	if (!prespFilters->Valid(row,col))
		return;

	for (int i=0; i<prespFilters->TotoalNumFilterResult(); i++)
	{
		sprintf(name,"%s_%d.his",pname,i);
		pfile = fopen(name, "w+");
		if (pfile != NULL)
		{
			fprintf(pfile, "%.2f\n", phist_clusters[i].min_bin);
			fprintf(pfile, "%.2f\n", phist_clusters[i].max_bin);
			fprintf(pfile, "%.2f\n", phist_clusters[i].delta);
			for (int j=0; j<phist_clusters[i].mx_pixel_histogram(row,col).count; j++)
				fprintf(pfile, "%.2f\n", phist_clusters[i].mx_pixel_histogram(row,col).pbins[j]);
			fclose(pfile);
		}
	}
}

void MCMC_Particle_Texture::CreateSoftClusteringMap(void)
{
	RF	rf;
	int	i,j,k;

	if (num_particle>0)
	{
		// initialization
		if (pmx_saliency_map != NULL)
			delete []pmx_saliency_map;
		pmx_saliency_map = new Raster<float>[num_particle];
		for (i=0; i<num_particle; i++)
			pmx_saliency_map[i].SetDimension(prespFilters->rows(),prespFilters->cols());

		rf.Set(0,num_particle-1,1);

		for (j=0; j<prespFilters->rows(); j++)
			for (i=0; i<prespFilters->cols(); i++)
			{
				for (k=0; k<num_particle; k++)
					rf.SetOneBin(k,exp(-DistanceToPar(j,i,k)));
				rf.normalize();
				for (k=0; k<num_particle; k++)
					pmx_saliency_map[k](j,i) = (float)rf.pdf(k);
			}
	}
}


void MCMC_Particle_Texture::Set(MCMC_FilterResp &respFilters, mcmcMatrix<int> &clst, char *pfile_name)
{
	Raster<int>		ras1,ras2;
	char			name[200];
	bool			bClusterExist = false;
	ifstream		_file;
	int				i,k;
	Raster<double>	*pras_result;
	double			delta;

	// initialization
	prespFilters = &respFilters;
	if (prespFilters->InColor())
		prespFilters->SetAllFilterBanksForClustering();
	else
		prespFilters->SetAllFilterBanks();

	mx_map_temp = clst;
	num_particle = mx_map_temp.GetMax()+1;

	if (pfile_name!=NULL)
	{
		sprintf(name,"%s_texture.cls",pfile_name);
		_file.open(name, ios::in);
		if (_file)
		{
			bClusterExist = true;
			_file.close();
		}
	}

	if (bClusterExist)
	{
		ReadBigMatrix(mx_map_temp,name);
		num_particle = mx_map_temp.GetMax()+1;
	}

	if (phist_clusters != NULL)
		delete []phist_clusters;
	phist_clusters = new MCMC_Histogram_Clusters[prespFilters->TotoalNumFilterResult()];
	vc_weights.SetRowVector(prespFilters->TotoalNumFilterResult());
	vc_weights.InitValue(1.0);

	if (!bClusterExist)
	{
		k = 0;
		pras_result = prespFilters->GetFirstResult(delta);
		while (pras_result != NULL)
		{
			phist_clusters[k++].Set(*pras_result,mx_map_temp,num_particle,window_size,delta);
			pras_result = prespFilters->GetNextResult(delta);
		}

		// soft clustering map
		double	dsum,dtemp;
		int		j;

		// EM algorithm
		EmSmt();

		pras_result = prespFilters->GetFirstResult(delta);
		for (i=0; i<prespFilters->TotoalNumFilterResult(); i++)
		{
			phist_clusters[i].Set(*pras_result,mx_map_temp,num_particle,window_size,delta,true);
			pras_result = prespFilters->GetNextResult(delta);
		}

		// EM algorithm
		EmSmt();

		pras_result = prespFilters->GetFirstResult(delta);
		for (i=0; i<prespFilters->TotoalNumFilterResult(); i++)
		{
			assert(pras_result != NULL);
			phist_clusters[i].Set(*pras_result,mx_map_temp,num_particle,window_size,delta,true);
			pras_result = prespFilters->GetNextResult(delta);
		}

		if (pmx_saliency_map != NULL)
			delete []pmx_saliency_map;
		pmx_saliency_map = new mcmcMatrix<float>[num_particle];
		for (k=0; k<num_particle; k++)
			pmx_saliency_map[k].SetDimension(prespFilters->rows(),prespFilters->cols());

		for (j=0; j<prespFilters->rows(); j++)
			for (i=0; i<prespFilters->cols(); i++)
			{
				dsum = 0.0;
				for (k=0; k<num_particle; k++)
				{
					dtemp = exp(-DistanceToPar(j,i,k));
					pmx_saliency_map[k](j,i) = (float)dtemp;
					dsum += dtemp;
				}
				for (k=0; k<num_particle; k++)
					pmx_saliency_map[k](j,i) = (float)(pmx_saliency_map[k](j,i)/dsum);
			}

		// save clustering map
		sprintf(name,"%s_texture.cls",pfile_name);
		WriteBigMatrix(mx_map_temp,name);

		for (k=0; k<num_particle; k++)
		{
			sprintf(name,"%s_texture_%d.smp",pfile_name,k);
			WriteBigMatrix(pmx_saliency_map[k], name);
		}
	}
	else
	{
		if (pmx_saliency_map != NULL)
			delete []pmx_saliency_map;
		pmx_saliency_map = new mcmcMatrix<float>[num_particle];
		for (k=0; k<num_particle; k++)
		{
			sprintf(name,"%s_texture_%d.smp",pfile_name,k);
			ReadBigMatrix(pmx_saliency_map[k], name);
		}
	}

	// save cluster histograms
	if (bprint_histograms)
	{
		int j;
		j = 0;
		for (i=0; i<prespFilters->TotoalNumFilterResult(); i++)
		{
			sprintf(name,"filter_%d",j++);
			phist_clusters[i].SaveToFile(name);
		}
	}

	// learn all the pseudo likelihood model paramters
	if (bto_learn_texture)
		LearnPLE4Particles(pfile_name);
}

void MCMC_Particle_Texture::EmSmt(void)
{
	RF						rf;
	int						i,j,k,m;
	double					dtemp;
	mcmcVector<double>			vc_dists;
	SimpleMatrix<Histogram>	mx_hist_temp;

	vc_dists.SetRowVector(prespFilters->TotoalNumFilterResult());
	rf.Set(0,num_particle-1,1);
	mx_hist_temp.SetDimension(num_particle,1);

	for (m=0; m<0; m++)
	{
		// setp one:
		// assign a cluster to each pixel and sample
		for (j=0; j<prespFilters->rows(); j++)
			for (i=0; i<prespFilters->cols(); i++)
			{
				for (k=0; k<num_particle; k++)
				{
					dtemp = DistanceToPar(j,i,k);
					rf.SetOneBin(k,exp(-dtemp));
				}
				k = (int)rf.SamplingNonSetCdf();
				mx_map_temp(j,i) = k;
			}
		// setp 2:
		for (i=0; i<prespFilters->TotoalNumFilterResult(); i++)
		{
			phist_clusters[i].ComputeMeanHist(mx_hist_temp,mx_map_temp);
			for (k=0; k<num_particle; k++)
				for (int ii=0; ii<phist_clusters[i].mx_cluster_histogram(k).count; ii++)
				phist_clusters[i].mx_cluster_histogram(k).pbins[ii] = phist_clusters[i].mx_cluster_histogram(k).pbins[ii]+
				(mx_hist_temp(k).pbins[ii]-phist_clusters[i].mx_cluster_histogram(k).pbins[ii])*0.1;
		}
	}

	// any two clusters are overlapping?
	mcmcMatrix<int>		mx_sign(num_particle,1);

	mx_sign.InitValue(1);
	for (i=0; i<num_particle; i++)
	{
		if (mx_sign(i)==1)
		{
			for (j=i+1; j<num_particle; j++)
			{
				if (mx_sign(j)==1)
				{
					for (m=0; m<prespFilters->TotoalNumFilterResult(); m++)
						vc_dists(m) = phist_clusters[m].DistanceBetweenPars(i,j);
					if (CombinedDist(vc_dists)<dist_to_merge)
						mx_sign(j) = 0;
				}
			}
		}
	}

	mcmcMatrix<int> mx_index(num_particle,1);
	int			ipar_temp;
	double		dist_temp;

	m = 0;
	for (i=0; i<num_particle; i++)
		if (mx_sign(i)==1)
			mx_index(i) = m++;

	// relabel
	for (j=0; j<prespFilters->rows(); j++)
		for (i=0; i<prespFilters->cols(); i++)
		{
			dist_temp = 1e90;
			ipar_temp = -1;

			for (k=0; k<num_particle; k++)
			{
				if (mx_sign(k)==1)
				{
					dtemp = DistanceToPar(j,i,k);
					if (dtemp<dist_temp)
					{
						ipar_temp = k;
						dist_temp = dtemp;
					}
				}
			}
			assert(ipar_temp>=0);
			mx_map_temp(j,i) = mx_index(ipar_temp);
		}

	num_particle = m;
}

mcmcVector<double> MCMC_Particle_Texture::GetMean(const int iparticle)
{
	mcmcVector<double> vc_par(1);

	// return particle number only
	vc_par(0) = (double)iparticle;
	return vc_par;
}

int	MCMC_Particle_Texture::ClosestParticle(mcmcVector<double> &mx_mean)
{
	return (int)mx_mean(0);
}

int	MCMC_Particle_Texture::CloserParticle(const mcmcVector<double> &mx_mean1, const mcmcVector<double> &mx_mean2, const int ipar)
{
	if ((int)mx_mean1.Data(0)==ipar)
		return 0;
	else
		return 1;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
MCMC_Particle_ColorTexture::MCMC_Particle_ColorTexture(const bool blearn)
:MCMC_Particle_Texture(blearn)
{
	// window size for computations of histograms
	pref_b_l = NULL;
	pref_b_u = NULL;
	pref_b_v = NULL;

	p_mx_syn = NULL;

	dist_to_merge = 0.35;
}


MCMC_Particle_ColorTexture::~MCMC_Particle_ColorTexture()
{
	Free();
}

void MCMC_Particle_ColorTexture::Free(void)
{
	MCMC_Particle_Texture::Free();

	if (p_mx_syn != NULL)			delete []p_mx_syn;
	p_mx_syn = NULL;

	if (pref_b_l != NULL)			delete []pref_b_l;
	pref_b_l = NULL;
	if (pref_b_u != NULL)			delete []pref_b_u;
	pref_b_u = NULL;
	if (pref_b_v != NULL)			delete []pref_b_v;
	pref_b_v = NULL;
}

void MCMC_Particle_ColorTexture::ComputeJacobs(void)
{
	mx_jacobs.SetDimension(prespFilters->rows(),prespFilters->cols());

	double log_delta;
	log_delta = log(pref_b_l->d)+log(pref_b_u->d)+log(pref_b_v->d);

	// get the jacobis term for comparable with that in R,G, and B
	double	pLUV[3],d;
	int		pRGB1[3],pRGB2[3];

	for (int j=0; j<prespFilters->rows(); j++)
		for (int i=0; i<prespFilters->cols(); i++)
		{
			pLUV[0] = prespFilters->ras_L(j,i);
			pLUV[1] = prespFilters->ras_U(j,i);
			pLUV[2] = prespFilters->ras_V(j,i);

			prespFilters->image_original.convert_LUV_RGB(pLUV,pRGB1);
			if (pLUV[0]>100)	pLUV[0] -= 1.0;
			else				pLUV[0] += 1.0;
			pLUV[1] += 1.0;
			pLUV[2] += 1.0;
			prespFilters->image_original.convert_LUV_RGB(pLUV,pRGB2);

			mx_jacobs(j,i) = log_delta;
			for (int t=0; t<3; t++)
			{
				d = fabs((float)(pRGB1[t]-pRGB2[t]));
				if (d<1e-3) d=1e-3;
				mx_jacobs(j,i) += log(d);
			}
		}
}


void MCMC_Particle_ColorTexture::LearnPLE4Particles(char *pfile_name)
{
	bool bresults_exist=false;

	if (p_mx_energymap != NULL)	delete []p_mx_energymap;
	if (p_mx_syn != NULL)		delete []p_mx_syn;
	if (pref_b_l != NULL)		delete []pref_b_l;
	if (pref_b_u != NULL)		delete []pref_b_u;
	if (pref_b_v != NULL)		delete []pref_b_v;

	p_mx_energymap	= new mcmcMatrix<double>[num_particle];
	p_mx_syn		= new Raster_Color<uchar>[num_particle];
	pref_b_l		= new Gibbs_B[num_particle];
	pref_b_u		= new Gibbs_B[num_particle];
	pref_b_v		= new Gibbs_B[num_particle];


	char	name[200];
	FILE	*pfile=NULL;
	if (pfile_name != NULL)
	{
		// reference data
		sprintf(name,"%s.ref",pfile_name);
		pfile = fopen(name,"r");
		if (pfile != NULL)
		{
			bresults_exist = true;
			fclose(pfile);
		}
	}

	int k,j;

	if (!bresults_exist)
	{
		prespFilters->SetAllFilterBanks();

		// learn all the ple parameters
		MCMC_Learn_PLE_Clusters learn_ple;

		learn_ple.Set(mx_map_temp,*prespFilters);
		learn_ple.LearnPLE4Clusters(pref_b_l,pref_b_u,pref_b_v,p_mx_energymap,p_mx_syn);
		vc_valid_cluster = learn_ple.vc_valid_clusters;

		// save all the results to disk
		if (pfile_name != NULL)
		{
			sprintf(name,"%s.ref",pfile_name);
			pfile = fopen(name,"w+");
			if (pfile != NULL)
			{
				// load precomputed results directly
				for (k=0; k<num_particle; k++)
				{
					pref_b_l[k].WriteToTxtFile(*pfile);
					pref_b_u[k].WriteToTxtFile(*pfile);
					pref_b_v[k].WriteToTxtFile(*pfile);
				}
				fclose(pfile);
			}
			for (k=0; k<num_particle; k++)
			{
				sprintf(name,"%s_%d.eng",pfile_name,k);
				WriteBigMatrix(p_mx_energymap[k],name);
				sprintf(name,"%s_syn_%d.ppm",pfile_name,k);
				WriteToPPM(p_mx_syn[k],name);
			}
		}

	}
	else
	{
		if (pfile_name != NULL)
		{
			// load precomputed results directly
			pfile = fopen(name,"r");
			for (k=0; k<num_particle; k++)
			{
				pref_b_l[k].ReadFromTxtFile(*pfile);
				pref_b_u[k].ReadFromTxtFile(*pfile);
				pref_b_v[k].ReadFromTxtFile(*pfile);
			}
			fclose(pfile);

			vc_valid_cluster.SetDimension(num_particle,1);

			for (k=0; k<num_particle; k++)
			{
				sprintf(name,"%s_%d.eng",pfile_name,k);
				ReadBigMatrix(p_mx_energymap[k],name);
				sprintf(name,"%s_syn_%d.ppm",pfile_name,k);
				ReadFromPPM(p_mx_syn[k],name);
				if (p_mx_energymap[k].size()>0)
					vc_valid_cluster(k) = true;
				else
					vc_valid_cluster(k) = false;
			}
		}
	}

	// compute jacobs at each position to make probability measure in (R,G,B) and (L,U,V) comparable
	ComputeJacobs();

	// reset salincy maps due to the small sizes of some clusters
	int dumy;
	for (k=0; k<num_particle; k++)
	{
		if (!vc_valid_cluster(k))
			pmx_saliency_map[k].InitValue(0.0);
		else
			dumy = k;
	}

	for (k=0; k<num_particle; k++)
		assert(pmx_saliency_map[k].size()==prespFilters->rows()*prespFilters->cols());

	double	sum;
	for (j=0; j<prespFilters->rows(); j++)
		for (int i=0; i<prespFilters->cols(); i++)
		{
			sum = 0.0;
			for (k=0; k<num_particle; k++)
				sum += pmx_saliency_map[k](j,i);
			if (sum>0.0)
			{
				for (k=0; k<num_particle; k++)
					if (vc_valid_cluster(k))
						pmx_saliency_map[k](j,i) = (float)(pmx_saliency_map[k](j,i)/sum);
			}
			else
				pmx_saliency_map[dumy](j,i) = 1.0;
		}
}


//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
MCMC_Particle_IntensityTexture::MCMC_Particle_IntensityTexture(const bool blearn)
:MCMC_Particle_Texture(blearn)
{
	p_mx_syn = NULL;
	pref_b_it = NULL;

	dist_to_merge = 0.15;
}

MCMC_Particle_IntensityTexture::~MCMC_Particle_IntensityTexture()
{
	Free();
}

void MCMC_Particle_IntensityTexture::Free(void)
{
	MCMC_Particle_Texture::Free();

	if (p_mx_syn != NULL)			delete []p_mx_syn;
	p_mx_syn = NULL;
	if (pref_b_it != NULL)			delete []pref_b_it;
	pref_b_it = NULL;
}


void MCMC_Particle_IntensityTexture::LearnPLE4Particles(char *pfile_name)
{
	bool bresults_exist=false;

	if (p_mx_energymap != NULL)	delete []p_mx_energymap;
	if (p_mx_syn != NULL)		delete []p_mx_syn;
	if (pref_b_it != NULL)		delete []pref_b_it;

	p_mx_energymap	= new mcmcMatrix<double>[num_particle];
	p_mx_syn		= new Raster<uchar>[num_particle];
	pref_b_it		= new Gibbs_B[num_particle];


	char	name[200];
	FILE	*pfile=NULL;
	if (pfile_name != NULL)
	{
		// reference data
		sprintf(name,"%s.ref",pfile_name);
		pfile = fopen(name,"r");
		if (pfile != NULL)
		{
			bresults_exist = true;
			fclose(pfile);
		}
	}

	int k,j;

	if (!bresults_exist)
	{
		prespFilters->SetAllFilterBanks();

		// learn all the ple parameters
		MCMC_Learn_PLE_Clusters learn_ple;

		learn_ple.Set(mx_map_temp,*prespFilters);
		learn_ple.LearnPLE4Clusters(pref_b_it,p_mx_energymap,p_mx_syn);
		vc_valid_cluster = learn_ple.vc_valid_clusters;

		// save all the results to disk
		if (pfile_name != NULL)
		{
			sprintf(name,"%s.ref",pfile_name);
			pfile = fopen(name,"w+");
			if (pfile != NULL)
			{
				// load precomputed results directly
				for (k=0; k<num_particle; k++)
					pref_b_it[k].WriteToTxtFile(*pfile);
				fclose(pfile);
			}
			for (k=0; k<num_particle; k++)
			{
				sprintf(name,"%s_%d.eng",pfile_name,k);
				WriteBigMatrix(p_mx_energymap[k],name);
				sprintf(name,"%s_syn_%d.ppm",pfile_name,k);
				WriteToPPM(p_mx_syn[k],name);
			}
		}

	}
	else
	{
		if (pfile_name != NULL)
		{
			// load precomputed results directly
			pfile = fopen(name,"r");
			for (k=0; k<num_particle; k++)
				pref_b_it[k].ReadFromTxtFile(*pfile);
			fclose(pfile);

			vc_valid_cluster.SetDimension(num_particle,1);

			for (k=0; k<num_particle; k++)
			{
				sprintf(name,"%s_%d.eng",pfile_name,k);
				ReadBigMatrix(p_mx_energymap[k],name);
				sprintf(name,"%s_syn_%d.ppm",pfile_name,k);
				ReadFromPPM(p_mx_syn[k],name);

				if (p_mx_energymap[k].size()>0)
					vc_valid_cluster(k) = true;
				else
					vc_valid_cluster(k) = false;
			}
		}
	}

	// compute jacobs at each position to make probability measure in (R,G,B) and (L,U,V) comparable
	ComputeJacobs();

	// reset salincy maps due to the small sizes of some clusters
	int dumy;
	for (k=0; k<num_particle; k++)
	{
		if (!vc_valid_cluster(k))
			pmx_saliency_map[k].InitValue(0.0);
		else
			dumy = k;
	}

	for (k=0; k<num_particle; k++)
		assert(pmx_saliency_map[k].size()==prespFilters->rows()*prespFilters->cols());


	double	sum;
	for (j=0; j<prespFilters->rows(); j++)
		for (int i=0; i<prespFilters->cols(); i++)
		{
			sum = 0.0;
			for (k=0; k<num_particle; k++)
				sum += pmx_saliency_map[k](j,i);
			if (sum>0.0)
			{
				for (k=0; k<num_particle; k++)
					if (vc_valid_cluster(k))
						pmx_saliency_map[k](j,i) = (float)(pmx_saliency_map[k](j,i)/sum);
			}
			else
				pmx_saliency_map[dumy](j,i) = 1.0;
		}

}
